@ MODE          IRQ ENABLED     IRQ DISABLED
@ ===========================================
@ Supervisor    0x13            0x93    	
@ User          0x10            0x90
@ System        0x1F            0x9F
@ IRQ           0x12            0x92

	.text
	.align	2
	.global	EnableCache
	.type	EnableCache, %function
mask:
  .word 0x1004
EnableCache:
  stmfd	sp!, {r0, r1}
  mrc p15, 0, r0, c1, c0, 0   @ read c1
  @ and r0, r0, mask            @ clear bits to be cleared
  ldr r1, mask
  orr r0, r0, r1           @ set bits to be set
  mcr p15, 0, r0, c1, c0, 0   @ write c1 
  ldmfd	sp, {r0, r1}
	mov pc, lr
	
	.align	2
	.global	KernelExit
	.type	KernelExit, %function
	 	
 	@ void KernelExit( td *task, req *request )
KernelExit:
 	@ we are in svc mode
    
    @ store parameters
    str r0, shared_td
    str r1, shared_req

    @ lr is the instruction after KernelExit() in Activate()
    @ store kernel state to kernel stack
    stmdb  sp!, {r4-r12, lr}
    
    @ switch to system mode with interrupts DISABLED
    msr cpsr_c, #0x9f
    
    @ load user stack pointer from task descriptor
    ldr r0, shared_td
    ldr sp, [r0, #4]
    
    @ load the spsr of the user task into shared spsr + enable interrupts in it
    @ with interrupts enabled, we'll be able to catch interrupts once user task starts
    ldr r1, [r0, #8]    
    bic r1, r1, #0xC0     @ ==> enables interrupts
    str r1, shared_spsr
    
    @ load user state from user stack
    ldmia sp!, {r0}    

    @ set aside the link register
    str r0, shared_lr
    
    ldmia  sp!, {r0-r12,lr}
        
    @ set aside r0 - we use it as scratch space :)
    str r0, shared_r0
        
    @ switch to supervisor mode with interrupts DISABLED
    msr cpsr_c, #0x93
    
    @ restore user task spsr to spsr_svc, for transfer by movs
    ldr r0, shared_spsr
    msr spsr, r0

    @ test mode in task descriptor
    ldr r0, shared_td
    ldr r0, [r0, #16]
    cmp r0, #1
    beq irq_exit

    @ SOFTWARE-INTERRUPT SPECIFIC

    @ load the return value into the return register
    ldr r0, shared_td
    ldr r0, [r0, #12]
    
    @ load back the link register
    ldr lr, shared_lr

    @ pc = lr (lr contains the pc of next user task instruction), cpsr = spsr
    
    movs pc, lr
    
irq_exit:
    @ HARDWARE-INTERRUPT SPECIFIC

    @ restore r0
    ldr r0, shared_r0    

    @ load back the link register
    ldr lr, shared_lr

    @ pc = lr (lr contains the pc of next user task instruction), cpsr = spsr
    
    subs pc, lr, #4
   
    .global KernelEnter

	@ Used registers:
 	@	r4 = the lr, which stores the next user task instruction address
KernelEnter:
    @ we are in supervisor mode (from swi), or irq mode (from hwi)
        
    @ store all the regsiters on the kernel stack to prevent user state corruption
    stmdb sp!, {r0-r12}
    
    @save the original mode    
    mrs r7, cpsr
    str r7, shared_orig_mode  
    
    @ load the mode number to see if we're in IRQ or SWI    
    mrs r4, cpsr
    and r4, r4, #0x1f
    cmp r4, #0x12
    beq irq
    
    @ SOFTWARE INTERRUPT-SPECIFIC:
    
    @ store first four parameters
    ldr r8, shared_req
    str r0, [r8, #4]
    str r1, [r8, #8]
    str r2, [r8, #12]
    str r3, [r8, #16]
    
    @ set mode in task descriptor
    ldr r0, shared_td
    mov r1, #0
    str r1, [r0, #16]
    
    @ get request system call number
    ldr r1, [lr, #-4]
    and r1, r1, #0xff
    str r1, [r8, #0]    
       
    @ store user program counter to shared location
    str lr, shared_lr
    
    @ switch to system mode, interrupts disabled
    msr cpsr_c, #0x9F
    
    @ store fifth parameter
    ldr r7, [ip, #0]
    str r7, [r8, #20]
    b continue
    
irq:
    
    @ HARDWARE INTERRUPT-SPECIFIC:
    
    @ set mode in task descriptor
    ldr r0, shared_td
    mov r1, #1
    str r1, [r0, #16]

    @ set system call number to 0xff to indicate hardware interrupt
    ldr r0, shared_req
    mov r1, #0xff
    str r1, [r0, #0]
    
    @ store user program counter to shared location
    str lr, shared_lr
    
    @ switch to system mode, interrupts disabled
    msr cpsr_c, #0x9F

continue: 
    @ store shared location to link register
    @ ldr lr, shared_lr
    @ lr_usr is the instruction after swi 
    @ sp_usr is a few bytes below where we left it (since Pass decrements sp)
    
    @load the saved registers in the svc/irq mode, containing the user state
    ldr r0, shared_orig_mode
    msr cpsr_c, r0
    
	  ldmia sp!, {r0-r12}    
	  msr cpsr_c, #0x9f
	  
    @ store user state on user stack
    stmdb sp!, {r0-r12, lr}
    ldr r0, shared_lr
    stmdb sp!, {r0}
    
    @ set aside sp
    str sp, shared_sp
    @ shared_sp is usr_sp AFTER r0-r12, lr pushed on
    
    @ switch to supervisor mode
    msr cpsr_c, #0x93

    @ load kernel state from kernel stack
    ldmia sp!, {r4-r12, lr}
    
    @ save the sp, spsr into the task descriptor of a user task
    ldr r0, shared_td
    ldr r1, shared_sp
    str r1, [r0, #4]
    mrs r1, spsr
    str r1, [r0, #8]

    @ lr is instruction after KernelExit() in Activate()
    @ (since lr reloaded from kernel stack)    
    
    mov pc, lr

@ global state used by the context switch
shared_td:
    .space 4
shared_req:
    .space 4
shared_lr:
    .space 4
shared_sp:
    .space 4
shared_spsr:
    .space 4
shared_orig_mode:
    .space 4
shared_r0:
    .space 4
